#define USBCON 0E0
#define UDINT 0xD8

#include <SD.h>
#include <avr/sleep.h>

#include "definitions.h"

#define USE_MIDI // uncomment if device is used in production

#ifdef USE_MIDI
#include <avr/usb_api.h>
#endif

File   archive;
String entries[MAX_ENTRIES];
int    numEntries = 0;
bool   usbConnected;

elapsedMillis lastRead;
elapsedMillis idleTime;
elapsedMillis usbIdle;
elapsedMillis voltageRead;
elapsedMillis lastSending;

void setup() {
  USBCON = USBCON | B00010000;
  #ifndef USE_MIDI
  Serial.begin(9600); 
  #endif
  usbIdle = 0;
  voltageRead = 0;
  turnOffUnusedSystems();
  flashSetup();
  debugLedSetup();
}

void loop() {	
  usbConnected = checkUsb();
  if (usbConnected) { readCommand(); }
  
  if (voltageRead > VOLTAGE_INTERVAL) {
    readAndSaveVoltage();
    voltageRead = 0;
  }
  
  int value = analogRead(FSR_PIN); // Les av trykkfølsom bryter
  if (value > WAKE_TRESHOLD) {
    usbIdle = 0;
    value = normalize(value);
    analogWrite(BLUE_PIN, map(value, 0, 1023, 64, 255));
    sendCurrentReading();
    if (lastRead > SAMPLE_TRESHOLD) { 
      if (numEntries >= MAX_ENTRIES) { saveEntriesToSd(); } // Bufferen er full
    
      String entry = millis();
      entry.concat(":");
      entry.concat(value);
      entries[numEntries] = entry;
      numEntries++;
      lastRead = 0;
    }
    idle();
  }
  else {
    analogWrite(BLUE_PIN, 0);
    saveEntriesToSd();
    idleTime = 0;
    #ifndef USE_MIDI
    if (usbIdle > 1000 * 60 * 10) { Serial.end(); } else { Serial.begin(9600); usbIdle = 0; }
    #endif
    while (idleTime < 100) {
     idle();
    }
  }
}

bool checkUsb() {
  return !(UDINT & B00000001);
}

int normalize(int value) { // kommer fra 0-1024, skal til 0-127
  return map(value, 0, 1023, 0, 127);
}

void readAndSaveVoltage() {
  File voltageFile = SD.open(VOLTAGE_FILE, FILE_WRITE);
  int reading      = analogRead(VOLTAGE_PIN);
  float voltage    = readVoltage();
  voltageFile.print(millis());
  voltageFile.print(":");
  voltageFile.print(reading);
  voltageFile.print(":");
  voltageFile.println(voltage);
  voltageFile.close();
}

float readVoltage() {
  return ((float) analogRead(VOLTAGE_PIN)) / ((float) VOLTAGE_42) * 4.18;
}

void saveEntriesToSd() {
  int i;
  for (i=0; i<numEntries; i++) {
    String entry = entries[i];
    flashWriteLine(entry);
  }
  numEntries = 0;
}

void sendCurrentReading() {
  if (!usbConnected) { return; }
  if (lastSending < 25) { return; }
  uint8_t value[] = {normalize(analogRead(FSR_PIN))};
  
  #ifdef USE_MIDI
  //usbMIDI.sendSysEx(1, value); 
  usbMIDI.sendNoteOn(value[0], 64, 0);
  delay(250);
  usbMIDI.sendNoteOff(value[0], 64, 0);
  #else
  Serial.println(value[0]);
  #endif
  lastSending = 0;
}

void exportEntries() {
  File entryFile = SD.open(ENTRY_FILE, FILE_READ);
  
  char r = entryFile.read();
  while (r != -1) {
    String line = "";
    while (r != '\n') { 
      line.concat(r);
      r = entryFile.read();
    }
    long time  = stringToLong(line.substring(0, line.indexOf(":")));
    int  value = stringToInt(line.substring(line.indexOf(":") + 1));
    
    long since = millis() - time;
    
    r = entryFile.read();
  }
  entryFile.close();
}

long stringToLong(String string) {
  int length = string.length() + 1;
  char array[length];
  string.toCharArray(array, length);
  return atol(array);
}

int stringToInt(String string) {
  int length = string.length() + 1;
  char array[length];
  string.toCharArray(array, length);
  return atoi(array);
}

void resetEntries() {
  SD.remove(ENTRY_FILE);
  SD.remove(VOLTAGE_FILE);
}

/*                                                                
Powersaving
*/

void idle() {
  set_sleep_mode(SLEEP_MODE_IDLE);
  noInterrupts();
  sleep_enable();
  interrupts();
  sleep_cpu();
  sleep_disable();
}


/*
Control
*/
void readCommand() {
  #ifdef USE_MIDI
  char command = readMidiCommand();
  #else
  char command = readSerialCommand();
  #endif
  
  if (command == 0) { return; }
  
  digitalWrite(11, HIGH);
  delay(50);
  digitalWrite(11, LOW);
  
  switch (command) {
    case 'C': sendCurrentReading(); break;
  }
}

#ifndef USE_MIDI
char readSerialCommand() {
  if (Serial.available() == 0) { return 0; }
  return Serial.read();
}
#endif

#ifdef USE_MIDI
char readMidiCommand() {
  if (!usbMIDI.read() || usbMIDI.getType() != 0 /* note on */) { return 0; }
  
  return 'C';
  
  int note     = usbMIDI.getData1();
  int velocity = usbMIDI.getData2();
  
  switch (note) {
    case 0: return 'C';
  }
  
  return 0;
}
#endif

// Setups
void debugLedSetup() {
  pinMode(11, OUTPUT);
  digitalWrite(11, LOW);
}

/*                                                                
SDkort			
*/

void flashSetup() {

  debug("Initializing SD card...");
  // On the Ethernet Shield, CS is pin 4. It's set as an output by default.
  // Note that even if it's not used as the CS pin, the hardware SS pin 
  // (10 on most Arduino boards, 53 on the Mega) must be left as an output 
  // or the SD library functions will not work. 
  pinMode(10, OUTPUT);
  if (!SD.begin(CHIPSELECT)) {
    debug("initialization failed!");
    return;
  }
  debug("initialization done.");
}

boolean flashOpenFile() {
  archive = SD.open(ENTRY_FILE, FILE_WRITE);
  return archive;
}

boolean flashWriteLine(String s) {
  if(flashOpenFile()){
    archive.println(s);
    archive.close();
    return true;
  }
  return false;
}

void turnOffUnusedSystems() {
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(16, OUTPUT);
  pinMode(17, OUTPUT);
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);
}

void debug(String text) {
  #ifndef USE_MIDI
  Serial.println(text);
  #endif
}